package lego_mindstorms;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;

public class main {
	static NXTRegulatedMotor right_m = Motor.A;
	static int counter = 0;
	static NXTRegulatedMotor left_m = Motor.B;
	static UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S1);
	static DifferentialPilot pilot = new DifferentialPilot(5.5, 5, Motor.A,
			Motor.B);
	static TouchSensor touch = new TouchSensor(SensorPort.S2);

	public static void run() {
		pilot.setTravelSpeed(25);
		pilot.forward();
	}

	public static void turn_right() {
		pilot.setTravelSpeed(15);
		counter++;
		pilot.travel(16);
		pilot.rotate(500);
		pilot.travel(10);
		pilot.rotate(700);		
		pilot.travel(10);
		pilot.rotate(400);
	}

	public static void turn_left() {
		pilot.setTravelSpeed(15);
		counter=0;
		pilot.travel(-5);
		pilot.rotate(-400);
		pilot.travel(7);
		pilot.rotate(-400);
		pilot.travel(7);
		pilot.rotate(-300);
		pilot.travel(5);
	}

	public static int getDistance() {
		int dist;
		dist = sonic.getDistance();

		if (dist > 100)
			dist = 0;

		return dist;
	}

	public static void adjustPosition() {
		pilot.rotate(-76.2);

	}

	public static void main(String[] args) {
		int dist = 0;
		boolean onoff = false;
		if (Button.ESCAPE.isDown()) {
			onoff = true;
		}

		pilot.setTravelSpeed(15);
		pilot.setRotateSpeed(120);
		// right_m.forward();
		// right_m.stop();
		while (onoff) {
			if (Button.ENTER.isDown()) {
				onoff = false;
			}

			dist = getDistance();
			LCD.drawString("Distance - " + dist, 0, 0);
			
			while (!touch.isPressed() && dist < 20) {
				LCD.drawString("Run forward - " + dist, 0, 0);
				run();
				dist = getDistance();
				if (dist < 5) {
					adjustPosition();
				}
			}

			if (dist >= 20) {
				if (counter < 4) {
					LCD.drawString("Turning right - " + dist, 0, 0);
					turn_right();
				} else {
					counter = 0;
					LCD.drawString("Too many turns", 0, 0);
					run();
				}
				
			}
			else {
				if (touch.isPressed()) {			
					LCD.drawString("Turning left - " + dist, 0, 0);
					turn_left();	
				} else {
					LCD.drawString("This should not happen :D", 0, 0);
				}
			}
			
		}

	}
}
